//
// Created by xiang on 22-12-20.
//

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <yaml-cpp/yaml.h>

#include "common/io_utils.h"
#include "fusion.h"
DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
DEFINE_string(config_yaml, "../config/mapping.yaml", "配置文件");

DEFINE_double(antenna_angle, 0.0, "RTK天线安装偏角（角度）");
DEFINE_double(antenna_pox_x, -0.25, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, 0.0, "RTK天线安装偏移Y");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::Fusion fusion(FLAGS_config_yaml);
    if (!fusion.Init()) {
        return -1;
    }

    //auto yaml = YAML::LoadFile(FLAGS_config_yaml);

    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
    
    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
          fusion.ProcessIMU(std::shared_ptr<sad::IMU>(new sad::IMU(imu)));                  
          usleep(1e3);
    })         
        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
            sad::GNSS gnss_convert = gnss;
            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, 0.0) || !gnss_convert.heading_valid_) {
                return false;
            }
            fusion.ProcessRTK(std::shared_ptr<sad::GNSS>(new sad::GNSS(gnss_convert)));
    })
        .SetOdomProcessFunc([&](const sad::Odom& odom) {
            fusion.ProcessOdom(std::shared_ptr<sad::Odom>(new sad::Odom(odom)));
    })
        .SetSimplePointCloudProcessFunc([&](const pcl::PointCloud<SimplePointType>::Ptr& cloud_ptr) -> bool  {
            fusion.ProcessPointCloud(cloud_ptr);
            return true;
    })
    .Go();

    LOG(INFO) << "done.";
    sleep(10);
    return 0;
}